﻿#include "meMotion.h"
#include <math.h>
#include "matrixd.h"
#include "meTransform.h"
#include "memath.h"

std::vector<std::vector<int>> MeMotion::lineBresenham(int *spos, int *epos, const uint &size) {
    //==================================
    // 算法实现 参考 https://www.geeksforgeeks.org/bresenhams-algorithm-for-3-d-line-drawing/
    std::vector<std::vector<int>> points;
    uint i = 0;

    // 计算delta 和 sign
    std::vector<int> d(size, 0); // delta
    std::vector<int> s(size, 0); // sign
    std::vector<int> pos(size, 0);
    for (i = 0; i < size; i++) {
        pos[i] = spos[i];
        d[i] = abs(epos[i] - spos[i]);
        if (epos[i] > spos[i])
            s[i] = 1;
        else
            s[i] = -1;
    }

    uint maxIndex = 0;
    int dmax = d[0];
    // find driving axis
    for (i = 1; i < size; i++) {
        if (d[i] > dmax) {
            maxIndex = i;
            dmax = d[i];
        }
    }

    // 初始偏差
    std::vector<int> p(size, 0);
    for (i = 0; i < size; i++) {
        if (i != maxIndex) {
            p[i] = 2 * d[i] - d[maxIndex];
        } else {
            p[i] = 0;
        }
    }

    // 生成插值点
    points.push_back(pos);
    while (pos[maxIndex] != epos[maxIndex]) {
        for (i = 0; i < size; i++) {
            if (i == maxIndex) {
                pos[i] += s[i];
            } else {
                if (p[i] >= 0) {
                    pos[i] += s[i];
                    p[i] += 2 * d[i] - 2 * d[maxIndex];
                } else {
                    p[i] += 2 * d[i];
                }
            }
        }
        points.push_back(pos);
    }

    return points;
}

std::vector<std::vector<double> > MeMotion::lineBresenham(double *sposF, double *eposF, const int factor, const unsigned int &size)
{
    //==================================
    // 算法实现 参考 https://www.geeksforgeeks.org/bresenhams-algorithm-for-3-d-line-drawing/
    std::vector<std::vector<double>> points;
    uint i = 0;

    // 启动和终点转成int后计算
    std::vector<int> spos(size, 0);
    std::vector<int> epos(size, 0);
    for (i = 0; i < size; i++) {
        spos[i] = lround(sposF[i] * factor);
        epos[i] = lround(eposF[i] * factor);
    }

    // 计算delta 和 sign
    std::vector<int> d(size, 0); // delta
    std::vector<int> s(size, 0); // sign
    std::vector<int> pos(size, 0);
    std::vector<double> posF(size, 0);  // 浮点
    for (i = 0; i < size; i++) {
        pos[i] = spos[i];
        posF[i] = sposF[i];
        d[i] = abs(epos[i] - spos[i]);
        if (epos[i] > spos[i])
            s[i] = 1;
        else
            s[i] = -1;
    }

    uint maxIndex = 0;
    int dmax = d[0];
    // find driving axis
    for (i = 1; i < size; i++) {
        if (d[i] > dmax) {
            maxIndex = i;
            dmax = d[i];
        }
    }

    // 初始偏差
    std::vector<int> p(size, 0);
    for (i = 0; i < size; i++) {
        if (i != maxIndex) {
            p[i] = 2 * d[i] - d[maxIndex];
        } else {
            p[i] = 0;
        }
    }

    // 生成插值点
    points.push_back(posF);
    double stepF = double(1)/factor;
    while (pos[maxIndex] != epos[maxIndex]) {
        for (i = 0; i < size; i++) {
            if (i == maxIndex) {
                pos[i] += s[i];
                posF[i] += stepF;
            } else {
                if (p[i] >= 0) {
                    pos[i] += s[i];
                    posF[i] += stepF;
                    p[i] += 2 * d[i] - 2 * d[maxIndex];
                } else {
                    p[i] += 2 * d[i];
                }
            }
        }
        points.push_back(posF);
    }

    return points;
}

void MeMotion::lineBresenham(const int *spos, const int *epos, const unsigned int &size,
                             MeMotion::lpInterpolationCB fCB, void *passObject, void *passData) {
    //==================================
    // 算法实现 参考 https://www.geeksforgeeks.org/bresenhams-algorithm-for-3-d-line-drawing/
    uint i = 0;

    // 计算delta 和 sign
    std::vector<int> d(size, 0); // delta
    std::vector<int> s(size, 0); // sign
    std::vector<int> pos(size, 0);
    std::vector<int> relativePos(size, 0);
    for (i = 0; i < size; i++) {
        pos[i] = spos[i];
        d[i] = abs(epos[i] - spos[i]);
        if (epos[i] > spos[i])
            s[i] = 1;
        else
            s[i] = -1;
    }

    uint maxIndex = 0;
    int dmax = d[0];
    // find driving axis
    for (i = 1; i < size; i++) {
        if (d[i] > dmax) {
            maxIndex = i;
            dmax = d[i];
        }
    }

    // 初始偏差
    std::vector<int> p(size, 0);
    for (i = 0; i < size; i++) {
        if (i != maxIndex) {
            p[i] = 2 * d[i] - d[maxIndex];
        } else {
            p[i] = 0;
        }
    }

    // 生成插值点
    if (fCB != nullptr) {
        fCB(passObject, passData, &pos, &relativePos);
    }
    while (pos[maxIndex] != epos[maxIndex]) {
        for (i = 0; i < size; i++) {
            if (i == maxIndex) {
                pos[i] += s[i];
                relativePos[i] = s[i];
            } else {
                if (p[i] >= 0) { // 移动一步
                    pos[i] += s[i];
                    relativePos[i] = s[i];
                    p[i] += 2 * d[i] - 2 * d[maxIndex];
                } else { // 不动
                    relativePos[i] = 0;
                    p[i] += 2 * d[i];
                }
            }
        }
        if (fCB != nullptr) {
            fCB(passObject, passData, &pos, &relativePos);
        }
    }
}

std::vector<std::vector<double>> MeMotion::linearPoints(double *spos, double *epos, const size_t count,
                                                        const size_t dimension) {
    std::vector<std::vector<double>> points;

    size_t i = 0, j = 0;

    std::vector<double> pos(size_t(dimension), 0.0);

    for (i = 0; i < count; i++) {
        for (j = 0; j < dimension; j++) {
            pos[j] = spos[j] + i * (epos[j] - spos[j]) / (count - 1);
        }
        points.push_back(pos);
    }

    return points;
}

std::vector<std::vector<double> > MeMotion::linearPointsWithStep(double *spos, double *epos, const double step, const size_t dimension)
{
    double lmax = 0.0;
    double l = 0;
    for (size_t i = 0; i < dimension; i++) {
        l = abs(epos[i] - spos[i]);
        if (l > lmax) {
            lmax = l;
        }
    }

    uint count = uint(lround(lmax/step));
    if (count < 2) count = 2;  // 只有起点和终点
    return MeMotion::linearPoints(spos, epos, count, dimension);
}

std::vector<std::vector<int>> MeMotion::arc2DPoints(int *current, int *target, int *center, bool isClockwiseArc) {
    int radius = lround(sqrt((current[0] - center[0]) * (current[0] - center[0]) +
                             (current[1] - center[1]) * (current[1] - center[1])));

    // 相对坐标
    int Xs = current[0] - center[0];
    int Ys = current[1] - center[1];
    int Xe = target[0] - center[0];
    int Ye = target[1] - center[1];

    std::vector<std::vector<int>> pointsOut = MeMotion::arc2DPoints(Xs, Ys, Xe, Ye, radius, isClockwiseArc);

    for (size_t i = 0; i < pointsOut.size(); i++) {
        pointsOut[i][0] += center[0];
        pointsOut[i][1] += center[1];
    }
    return pointsOut;
}

std::vector<std::vector<double>> MeMotion::arc2DPoints(double *current, double *target, double *center,
                                                       const int factor, bool isClockwiseArc) {
    std::vector<std::vector<double>> outPoints;
    // 相对坐标
    int Xs = lround((current[0] - center[0]) * factor);
    int Ys = lround((current[1] - center[1]) * factor);
    int Xe = lround((target[0] - center[0]) * factor);
    int Ye = lround((target[1] - center[1]) * factor);
    int radius = lround(sqrt(Xs * Xs + Ys * Ys));

    std::vector<std::vector<int>> points = MeMotion::arc2DPoints(Xs, Ys, Xe, Ye, radius, isClockwiseArc);

    // add center
    // 对点进行转换
    std::vector<double> p(2, 0);
    for (size_t i = 0; i < points.size(); i++) {
        p[0] = center[0] + points[i][0] / factor;
        p[1] = center[1] + points[i][1] / factor;

        outPoints.push_back(p);
    }

    return outPoints;
}

std::vector<std::vector<int>> MeMotion::arc2DPoints(const int &Xs, const int &Ys, const int &Xe, const int &Ye,
                                                    const int &radius, bool isClockwiseArc) {
    std::vector<std::vector<int>> pointsOut;

    std::vector<std::vector<int>> points = circlePoints(radius);

    size_t i = 0;
    size_t count = 0;
    bool isStarted = false;
    // std::cout << "start:" << Xs <<"," << Ys << std::endl;
    // std::cout << "end:" << Xe <<"," << Ye << std::endl;
    if (isClockwiseArc) {
        // 逆弧，从开始到结束比较
        i = points.size() - 1;
        while (count < points.size()) {
            // std::cout << count <<":" << points[i][0] <<"," << points[i][1] << std::endl;
            //            if ((false == isStarted) && Xs == points[i][0] && Ys == points[i][1]) {
            if ((false == isStarted) && isNearArcPoint(points[i][0], points[i][1], Xs, Ys, isClockwiseArc)) {
                isStarted = true;
            }
            if (isStarted) {
                pointsOut.push_back(std::vector<int>{points[i][0], points[i][1]});
                count++;
            }
            //            if (isStarted && Xe == points[i][0] && Ye == points[i][1]) {
            if (isStarted && isNearArcPoint(points[i][0], points[i][1], Xe, Ye, isClockwiseArc)) {
                pointsOut.push_back(std::vector<int>{points[i][0], points[i][1]});
                break;
            }
            if (i == 0) {
                i = points.size();
            }
            i--;
        }
    } else {
        // 顺弧，从开始到结束比较
        i = 0;
        while (count < points.size()) {
            if (i == points.size()) {
                i = 0;
            }
            if ((false == isStarted) && isNearArcPoint(points[i][0], points[i][1], Xs, Ys, isClockwiseArc)) {
                isStarted = true;
            }
            if (isStarted) {
                pointsOut.push_back(std::vector<int>{points[i][0], points[i][1]});
                count++;
            }
            if (isStarted && isNearArcPoint(points[i][0], points[i][1], Xe, Ye, isClockwiseArc)) {
                pointsOut.push_back(std::vector<int>{points[i][0], points[i][1]});
                break;
            }
            i++;
        }
    }

    return pointsOut;
}

int MeMotion::arc3DPoints(double p1[], double p2[], double p3[], const int factor,
                          std::vector<std::vector<double>> &outPoints) {
    // 计算p1到p2的单位向量和p1到p3的单位向量
    //  检查点是否相同
    double v1[3], v2[3];
    MeMath::subtract(p2, p1, v1);
    MeMath::subtract(p3, p1, v2);
    double nv1 = MeMath::norm(v1);
    double nv2 = MeMath::norm(v2);
    if (MeMath::isZero(nv1) || MeMath::isZero(nv2)) {
        // 重合的点
        return -1;
    }

    double v3[3];
    MeMath::cross(v1, v2, v3);
    if (MeMath::isZeroVec3d(v3)) {
        return -2;
    }
    MeMath::normalize(v3);

    // 计算新坐标系UVW轴
    double u[3] = {v1[0] / nv1, v1[1] / nv1, v1[2] / nv1};
    double *w = v3;
    double v[3];

    MeMath::cross(w, u, v); // z*x = y

    // 计算投影 P1 P2 P3 => A B C (uvw 坐标下)  A(0, 0), B(bx, 0), C(cx, cy)
    double bx = MeMath::dot(v1, u);
    double cx = MeMath::dot(v2, u);
    double cy = MeMath::dot(v2, v);

    // 计算圆心
    double h = ((cx - bx / 2) * (cx - bx / 2) + cy * cy - (bx / 2) * (bx / 2)) / (2 * cy);

    double radius = sqrt(bx * bx / 4 + h * h);
    Matrixd m(4, 4); // 转转矩阵， 以 A 为 0点
    m.put(0, 0, u[0]);
    m.put(1, 0, u[1]);
    m.put(2, 0, u[2]);
    m.put(3, 0, 0); // col 1
    m.put(0, 1, v[0]);
    m.put(1, 1, v[1]);
    m.put(2, 1, v[2]);
    m.put(3, 1, 0); // col 2
    m.put(0, 2, w[0]);
    m.put(1, 2, w[1]);
    m.put(2, 2, w[2]);
    m.put(3, 2, 0); // col 3
    m.put(0, 3, p1[0]);
    m.put(1, 3, p1[1]);
    m.put(2, 3, p1[2]);
    m.put(3, 3, 1); // col 4

    // Matrixd invM(4,4);
    // MeTransform::invertRotMatrix(m, invM);

    ////
    // A-> C点的逆弧算法，坐标点都是以mm为单位的，弧度单步计算需要的是整型，这里扩大一下倍数
    int iRadius = lround(radius * factor);
    // int iA[2] = {0, 0};
    // int iC[2] = {lround(cx * factor), lround(cy * factor)};
    int iCenter[2] = {lround((bx / 2) * factor), lround(h * factor)};

    // std::vector<std::vector<int> > points = arc2DPoints(iA, iC, iCenter, true);

    // 相对坐标
    int Xs = lround(-(bx / 2) * factor);
    int Ys = lround(-h * factor);
    int Xe = lround((cx - bx / 2) * factor);
    int Ye = lround((cy - h) * factor);

    std::vector<std::vector<int>> points = MeMotion::arc2DPoints(Xs, Ys, Xe, Ye, iRadius, true);

    // 对点进行转换
    for (size_t i = 0; i < points.size(); i++) {
        std::vector<double> p(3, 0);
        double x = (iCenter[0] + points[i][0]);
        double y = (iCenter[1] + points[i][1]);
        p[0] = (u[0] * x + v[0] * y) / factor + p1[0];
        p[1] = (u[1] * x + v[1] * y) / factor + p1[1];
        p[2] = (u[2] * x + v[2] * y) / factor + p1[2];
        outPoints.push_back(p);
    }

    return 0;
}

std::vector<std::vector<int>> MeMotion::circlePoints(int radius) {
    std::vector<std::vector<int>> points;

    int x, y, p;
    x = 0;
    y = radius;
    p = 3 - 2 * radius;

    size_t baseCount = 0;
    while (x < y) {
        points.push_back(std::vector<int>{x, y});
        baseCount++;
        if (p < 0)
            p = p + 4 * x + 6;
        else {
            p = p + 4 * (x - y) + 10;
            y -= 1;
        }
        x += 1;
    }
    if (x == y) points.push_back(std::vector<int>{x, y});

    // 2 补全 (x, y) => (y, x)
    size_t i = 0;
    for (i = baseCount - 1; i > 0; i--) {
        points.push_back(std::vector<int>{points[i][1], points[i][0]});
    }
    // 3 (x,y) => (y, -x)
    for (i = 0; i < baseCount; i++) {
        ;
        points.push_back(std::vector<int>{points[i][1], -points[i][0]});
    }
    if (x == y) points.push_back(std::vector<int>{y, -x});
    // 4 (x,y) => (x, -y)
    for (i = baseCount - 1; i > 0; i--) {
        points.push_back(std::vector<int>{points[i][0], -points[i][1]});
    }
    // 5 (x,y) => (-x, -y)
    for (i = 0; i < baseCount; i++) {
        points.push_back(std::vector<int>{-points[i][0], -points[i][1]});
    }
    if (x == y) points.push_back(std::vector<int>{-x, -y});

    // 6 (x,y) => (-y, -x)
    for (i = baseCount - 1; i > 0; i--) {
        points.push_back(std::vector<int>{-points[i][1], -points[i][0]});
    }
    // 7 (x,y) => (-y, x)
    for (i = 0; i < baseCount; i++) {
        points.push_back(std::vector<int>{-points[i][1], points[i][0]});
    }
    if (x == y) points.push_back(std::vector<int>{-y, x});
    // 8 (x,y) => (-x, y)
    for (i = baseCount - 1; i > 0; i--) {
        points.push_back(std::vector<int>{-points[i][0], points[i][1]});
    }

    return points;
}

int MeMotion::circle3DCenter(const double p1[3], const double p2[3], const double p3[3], double center[3]) {
    // 计算p1到p2的单位向量和p1到p3的单位向量
    //  检查点是否相同
    double v1[3], v2[3];
    MeMath::subtract(p2, p1, v1);
    MeMath::subtract(p3, p1, v2);
    double nv1 = MeMath::norm(v1);
    double nv2 = MeMath::norm(v2);
    if (MeMath::isZero(nv1) || MeMath::isZero(nv2)) {
        // 重合的点
        return -1;
    }

    double v3[3];
    MeMath::cross(v1, v2, v3);
    if (MeMath::isZeroVec3d(v3)) {
        return -2;
    }
    MeMath::normalize(v3);

    // 计算新坐标系UVW轴
    double u[3] = {v1[0] / nv1, v1[1] / nv1, v1[2] / nv1};
    double *w = v3;
    double v[3];

    MeMath::cross(w, u, v); // z*x = y

    // 计算投影 P1 P2 P3 => A B C (uvw 坐标下)  A(0, 0), B(bx, 0), C(cx, cy)
    double bx = MeMath::dot(v1, u);
    double cx = MeMath::dot(v2, u);
    double cy = MeMath::dot(v2, v);

    // 计算圆心
    double h = ((cx - bx / 2) * (cx - bx / 2) + cy * cy - (bx / 2) * (bx / 2)) / (2 * cy);

    for (int i = 0; i < 3; i++) {
        center[i] = p1[i] + (bx / 2) * u[i] + h * v[i];
    }

    return 0;
}

std::vector<std::vector<long>> MeMotion::arc2DInterpolationPoints(long *current, long *target, long *center,
                                                                  bool isClockwiseArc, long step) {
    std::vector<std::vector<long>> points;

    // ![step0] 初始化相对坐标和偏差f
    int f = 0; // 迭代偏差
    // 计算相对坐标
    long Xs = current[0] - center[0];
    long Ys = current[1] - center[1];

    long Xe = target[0] - center[0];
    long Ye = target[1] - center[1];

    long Xi = Xs;
    long Yi = Ys;

    // ![step1] 计算运动方向 +X,-X, +Y, -Y
    // 先算逆圆弧 NR
    int8_t signXs = -mcSign(Yi); // 起点方向
    int8_t signYs = mcSign(Xi);
    int8_t signXe = -mcSign(Ye); // 终点方向
    int8_t signYe = mcSign(Xe);
    int8_t signXp = mcSign(Xe - Xi);
    int8_t signYp = mcSign(Ye - Yi);

    // 顺圆弧需要调整 SR
    if (isClockwiseArc) {
        signXs = -signXs;
        signYs = -signYs;
        signXe = -signXe;
        signYe = -signYe;
    }
    // ![step1]

    // ![step2] 边界判断
    if (signXs == 0) {
        signXs = -mcSign(Xi);
    }
    if (signYs == 0) {
        signYs = -mcSign(Yi);
    }
    if (signXe == 0) {
        signXe = mcSign(Xe);
    }
    if (signYe == 0) {
        signYe = mcSign(Ye);
    }
    // ![step2]

    while (true) {
        // ![step3] 终止条件1,2
        // 终点不在同一象限或者走向相反，走STEP4，否则走STEP6
        if (signXs != signXe || signYs != signYe || signXs != signXp || signYs != signYp) {
            // ![step3]
            while (true) {
                // ![step4]
                arcStepOne(f, signXs, signYs, Xi, Yi);
                std::vector<long> pos(2, 0);
                pos[0] = center[0] + Xi;
                pos[1] = center[1] + Yi;
                points.push_back(pos);
                // ![step4]

                // ![step5] 跨弧度要调整参数
                if (Xi == 0) {
                    signYs = -signYs;
                    signYp = mcSign(Ye - Yi);
                    break;
                } else if (Yi == 0) {
                    signXs = -signXs;
                    signXp = mcSign(Xe - Xi);
                    break;
                }
                // ![step5]
            }
        } else { // 满足条件1,2   最后一个象限内
            do {
                // ![step6]
                arcStepOne(f, signXs, signYs, Xi, Yi);
                std::vector<long> pos(2, 0);
                pos[0] = center[0] + Xi;
                pos[1] = center[1] + Yi;
                points.push_back(pos);
                // ![step6]

                // ![step7] 结束条件
            } while (Xi != Xe || Yi != Ye);
            // ![step7]

            break;
        }
    }

    return points;
}

void MeMotion::arcStepOne(int &f, int8_t &signXs, int8_t &signYs, long &Xi, long &Yi) {
    int fx = f + 2 * signXs * Xi + 1;
    int fy = f + 2 * signYs * Yi + 1;
    int dF = qAbs(fy) - qAbs(fx);

    if (dF >= 0) { // X dir
        f = fx;
        Xi = Xi + signXs;
    } else {
        f = fy;
        Yi = Yi + signYs;
    }
}

bool MeMotion::isNearArcPoint(const int &x, const int &y, const int &tx, const int &ty, const bool &isClockwiseArc) {
    if (isClockwiseArc) {
        if (tx > 0 && ty >= 0) { // 1 quadrant
            if (x <= tx && y >= ty) return true;
        } else if (tx <= 0 && ty > 0) { // 2 quadrant
            if (x <= tx && y <= ty) return true;
        } else if (tx < 0 && ty <= 0) { // 3 quadrant
            if (x >= tx && y <= ty) return true;
        } else {
            if (x >= tx && y >= ty) return true;
        }
    } else {
        if (tx >= 0 && ty > 0) { // 1 quadrant
            if (x >= tx && y <= ty) return true;
        } else if (tx < 0 && ty >= 0) { // 2 quadrant
            if (x >= tx && y >= ty) return true;
        } else if (tx <= 0 && ty < 0) { // 3 quadrant
            if (x <= tx && y >= ty) return true;
        } else {
            if (x <= tx && y <= ty) return true;
        }
    }
    return false;
}
